package com.googlecode.grs.robot;

import java.awt.Color;
import java.awt.Graphics;
import java.awt.geom.Point2D.Double;

import com.googlecode.grs.core.GRTObject;

/**
 * A Wheel is an abstraction of wheel position integration using its velocity.
 * The API is almost entirely package protected because they only functionality
 * they need expose is their 2D position to the DifferentialDrivenBot.
 * 
 * A DifferentialDrivenRobot interacts with a Wheel in that it provides the
 * Wheel its heading, acting as a 'frame'.
 * 
 * This class is a <code>DrawnRobotComponent</code>, showing how pathing can be
 * abstracted without the SimulationEnvironment.
 * 
 * 
 * The theoretical problem with individual integration is that the distance
 * between each wheel cannot be conserved without compensation, but in practice
 * this is a negligible problem.
 * 
 * @author ajc
 * 
 */
public class Wheel extends GRTObject implements DrawnRobotComponent, Moveable {

	public static final double TIMESTEP = 0.001;

	private final Moveable frame; // provides robot orientation
	private double x;
	private double y;
	private double velocity = 0; // velocity indicates the rate of movement

	private boolean running = false; // indicates if the Wheel is integrating

	/**
	 * A wheel is constructed given 2D position
	 * 
	 * @param x
	 * @param y
	 */
	Wheel(Moveable frame, double x, double y) {
		this.frame = frame;
		this.x = x;
		this.y = y;
	}

	/**
	 * Starts calculating wheel position
	 */
	void startIntegration() {
		start();
	}

	/**
	 * Calculates wheel position and position history on a regular clock.
	 * Position is integrated given commanded velocity
	 */
	public void run() {
		running = true;
		while (running) {
			// increment position by the time that has elapsed and velocity
			x += Math.cos(frame.getHeading()) * velocity * TIMESTEP;
			y += Math.sin(frame.getHeading()) * velocity * TIMESTEP;

			try {
				sleep(1);
			} catch (InterruptedException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
		}
	}
	
	/**
	 * 
	 * @return wheel velocity
	 */
	public double getVelocity(){
		return velocity;
	}

	/**
	 * Commands this wheel to drive at a specified velocity
	 * 
	 * @param velocity
	 *            new velocity for wheel drive, any real number
	 */
	void setVelocity(double velocity) {
		this.velocity = velocity;
	}

	/**
	 * Sets wheel position. Should only be used for 'axle' length correction.
	 * 
	 * @param x
	 *            x component of new2D position
	 * @param y
	 *            y component of new2D position
	 */
	void setPosition(double x, double y) {// TODO not public
		this.x = x;
		this.y = y;
	}

	/**
	 * 
	 * @return x component of wheel's position
	 */
	public double getX() {
		return x;
	}

	/**
	 * 
	 * @return y component of wheel's position
	 */
	public double getY() {
		return y;
	}

	@Override
	/**
	 * Draws this robot, called by a <code>DrawnRobot</code>
	 * @param g
	 */
	public void draw(Graphics g) {
		// draw only position
		g.setColor(Color.BLUE);
		g.fillRect((int) x - 2, (int) y - 2, 4, 4);// TODO magic numbers

		// testing
		// g.setColor(Color.black);
		// g.drawString(name2, (int) x, (int) y - 10);
		// g.drawString("heading:" + Math.toDegrees(frame.getHeading()), (int)
		// x,
		// (int) y);
		// g.drawString("X: " + x, (int) x, (int) y + 10);
		// g.drawString("Y: " + y, (int) x, (int) y + 20);
	}

	@Override
	public double getHeading() {
		return frame.getHeading();
	}

	@Override
	public void setPosition(double x, double y, double angle) {
		// TODO Auto-generated method stub

	}

	@Override
	public void setPosition(Double position, double angle) {
		// TODO Auto-generated method stub

	}
	
	public void halt(){
		setVelocity(0);
	}

}
